﻿#include "DGlove.h"
#include <QTimer>
#include <QMessageBox>
#include <QXmlStreamWriter>
#include <QDomElement>

namespace vkbrd
{
DG5Glove::DG5Glove () : 
buffer(1)
{
	gloveId = -1;
}

DG5Glove::~DG5Glove()
{
}

bool DG5Glove::openGloveComPort()
{
	try
	{
		assert(ioThread.isNull());
		ioThread = new QtAsioThread(38400, portName.toStdString());
		bool success = true;
		success &= connect(ioThread, SIGNAL(receivedData(unsigned char*, int)), this, SLOT(messageRecived(unsigned char *, int )));
		success &= connect(this, SIGNAL(dispatchWriteToPort(unsigned char)), ioThread, SLOT(writeToPort(unsigned char)));
		success &= connect(this, SIGNAL(stopIOThread()), ioThread, SLOT(stop()));
		assert(success);

		ioThread->start();
		QTimer::singleShot(0, ioThread, SLOT(pollLoop()));

		ioThread->writeToPort(eStartCommand);
		ioThread->writeToPort(eSetLowPassOff);
		
		return true;
	}
	catch (std::exception& _e)
	{
		QMessageBox msgBox;
		msgBox.setText(QString(tr("Error while creating port connection: ")) + _e.what());
		msgBox.exec();

		return false;
	}
}

void DG5Glove::closeGloveComPort()
{
	if (!ioThread.isNull())
	{
		emit stopIOThread();
		ioThread->wait();
		delete ioThread;
	}
}

void DG5Glove::sendGloveCommand(unsigned char _command)
{
	emit(dispatchWriteToPort(_command));
}

void DG5Glove::messageRecived(unsigned char *_msg, int _bytesCount)
{
	for (int i = 0; i < _bytesCount; ++i)
		if(buffer.pushRecivedByte(_msg[i]))
		{
			emit rawDataReceived(gloveId, buffer.getAvgRead());
			if(fingerCalibration.updateBoolFingerState(currentFingerState, buffer.getAvgRead()))
				emit fingerStateChanged(gloveId, currentFingerState);
		}
}

bool DG5Glove::connected()
{
	return !ioThread.isNull();
}

const QString DG5Glove::gloveXmlElementName("glove");
const QString gloveIdXmlAttributeName("gloveId");
const QString glovePortXmlAttributeName("port");

void DG5Glove::serialize( QXmlStreamWriter* _writer )
{
	_writer->writeStartElement(gloveXmlElementName);
	_writer->writeAttribute(gloveIdXmlAttributeName, QString::number(gloveId));
	_writer->writeAttribute(glovePortXmlAttributeName, portName);

	fingerCalibration.serialize(_writer);

	_writer->writeEndElement();
}

const QString calibrationDataXmlElement("calibrationData");

void DG5Glove::deserialize( QDomElement& _gloveElement )
{
	gloveId = (_gloveElement.attribute(gloveIdXmlAttributeName)).toInt();
	portName = _gloveElement.attribute(glovePortXmlAttributeName);

	fingerCalibration.deserialize(_gloveElement.firstChildElement(calibrationDataXmlElement));
}

void DG5Glove::calibrate( BoolFingers _fingers )
{
	if (this->connected())
		fingerCalibration.calibrateMinMax(_fingers, buffer.getAvgRead());
	///@todo emit new calibration data
	emit calibrationChanged(gloveId, fingerCalibration);
}

void DG5Glove::resetCalibration()
{
	fingerCalibration.reset();	
}

void GloveBuffer::recalculateAvarageRead()
{
	gIntFingers sum;

	for (int i = 0; i < bufferSize; ++i)
		sum += lastFingersReads[i];

	avgGloveRead = sum / bufferSize;
}

void GloveBuffer::processMessage()
{
	gIntFingers newData;
	for (int i = 0; i < 5; ++i)
	{
		int hiBits = incomingData[2*i] & e5BitMask;
		int loBits = incomingData[2*i+1] & e5BitMask;

		lastFingersReads[fingerReadsIndex][i] = (hiBits << 5) | loBits;
	}
	fingerReadsIndex = (fingerReadsIndex+1) % bufferSize;
	recalculateAvarageRead();
}

GloveBuffer::GloveBuffer( int _initialSize )
{
	incomingIndex = 0;
	bufferSize = _initialSize;
	for(int i = 0; i < bufferSize; ++i)
		lastFingersReads.append(gIntFingers());
	fingerReadsIndex = 0;
}

bool GloveBuffer::pushRecivedByte( unsigned char _byte )
{
	if (incomingIndex == 0) //nothing in incoming buffer
	{
		if (eStartBit & _byte) //good start, write to buffer
			incomingData[incomingIndex++] = _byte;
	}
	else if (incomingIndex == messageLenght-1)
	{
		if (_byte == eEndByte)
		{
			incomingData[incomingIndex] = _byte;
			processMessage();					
			incomingIndex = 0;
			return true;
		}
		//else wrong end byte, discard whole message
		incomingIndex = 0;
	}
	else
	{
		incomingData[incomingIndex++] = _byte;
	}
	return false;
}

const QString fingerXmlElementName("finger");
const QString minXmlAttributeName("min");
const QString maxXmlAttributeName("max");

void FingerCalibration::serialize( QXmlStreamWriter* _writer )
{
	_writer->writeStartElement(calibrationDataXmlElement);
	for(int i = 0; i < fingerCount; ++i)
	{
		_writer->writeStartElement(fingerXmlElementName);
		_writer->writeAttribute(minXmlAttributeName, QString::number(minMax[i].minPress));
		_writer->writeAttribute(maxXmlAttributeName, QString::number(minMax[i].maxPress));
		_writer->writeEndElement();
	}
	_writer->writeEndElement();
}

void FingerCalibration::deserialize( QDomElement& _calibrationElement )
{
	QDomElement fingerElement = _calibrationElement.firstChildElement(fingerXmlElementName);
		for (int i = 0;
			!fingerElement.isNull(); 
			fingerElement = fingerElement.nextSiblingElement(fingerXmlElementName), ++i) 
		{
			if (i >= fingerCount)
				break;
			
			minMax[i].minPress = fingerElement.attribute(minXmlAttributeName).toInt();
			minMax[i].maxPress = fingerElement.attribute(maxXmlAttributeName).toInt();
		}
}

bool FingerCalibration::updateBoolFingerState( BoolFingers& _prevState, const gIntFingers& _intFingers )
{
	bool change = false;
	for (int i = 0; i < fingerCount; ++i)
	{
		if(!_prevState(i) && _intFingers[i] > minMax[i].maxPress)
		{
			_prevState.set(i, true);
			change = true;
		}
		else if (_prevState(i) && _intFingers[i] < minMax[i].minPress)
		{
			_prevState.set(i, false);
			change = true;
		}
		// get rid of uncalibrated data
		else if (minMax[i].minPress == 0 && minMax[i].maxPress == glovePrecision)
		{
			_prevState.set(i, false);
		}
	}
	return change;
}

void FingerCalibration::calibrateMinMax( BoolFingers _fingers, gIntFingers& _fingerReads )
{
	for (int i =0; i < fingerCount; ++i)
	{
		if (_fingers(i) && minMax[i].maxPress > _fingerReads[i])
			//lower maxPress
			minMax[i].maxPress = _fingerReads[i] - glovePrecision*0.05f;
		else if(!_fingers(i) && minMax[i].minPress < _fingerReads[i] )
			minMax[i].minPress = _fingerReads[i] + glovePrecision*0.05f;

		if (minMax[i].minPress > minMax[i].maxPress)
			resetFinger(i);
	}
}

} //namespace vkbrd